#ifndef _DIFF_DRIVE_VEHICLE_H_
#define _DIFF_DRIVE_VEHICLE_H_

#include <dispatch_system/vehicle.h>

namespace moying
{
namespace navigation
{
    class DiffDriveVehicle: public Vehicle
    {
        public:
            DiffDriveVehicle();
            DiffDriveVehicle(std::string type, const std::string &name);
            DiffDriveVehicle(std::string type, const std::string &name, unsigned int id);
            virtual ~DiffDriveVehicle();
            virtual bool isOmniVehicle();
            virtual void setMovePatterns();
            virtual double getTheta(int dir);
            // virtual double getVelocity(int sm);
            virtual void getNearNodes(const VehicleState &state, std::vector<Node> &nodes);
            virtual void constructPlanFromGraphPath(MoyingMapPtr moyingmap, const VehicleState &start, const VehicleState &goal, const std::vector<Node> &node_path, PlanningResult &result);
            bool getPathFootprintOccupiedCells(MoyingMapPtr moyingmap, const NodePtr &a, const NodePtr &b, std::map<int, FootprintCell> &footprint_cells);
            virtual unsigned int getStartNodeIndex(const VehicleState &start_state, char &sm);
            virtual unsigned int getGoalNodeIndex(const VehicleState &start_state, char &sm);
            // virtual void recalculatePathPoint(MoyingMapPtr moyingmap, float cur_theta,float cur_x,float cur_y,MovePatternState cur_state, float cell_theta,
            //         float cell_nav_x,float cell_nav_y,Node& node,std::chrono::system_clock::time_point last_time,PlanningResult &result);

        private:
            /**
             * @brief 均匀化路径段上的路径点,使路径点间的距离和到达时间都相等
             * @param [IN] moyingmap        地图指针
             * @param [IN] nodes            路径点
             * @param [IN] start_time       路径首节点时刻
             * @param [IN] start_state      起始状态
             * @param [IN] goal_state       目标状态
             * @param [in] start_index      要均匀化路径段的起始索引
             * @param [in] end_index        要均匀化路径段的结束索引
             * @param [OUT] path            均匀化后的路径
             * @param [OUT] collide_index   碰撞的索引 
             * @return  成功或失败
             */ 
            bool equalizePathPoint(MoyingMapPtr moyingmap,const std::vector<Node>& nodes,
                std::chrono::system_clock::time_point start_time,VehicleState start_state,VehicleState goal_state,
                int start_index,int end_index,std::vector<VehicleState> &path,int& collide_index);

            bool getLineSegmentOfNodePath(const std::vector<Node> &nodes,int start_index,int& end_index);
    };
}
}
#endif // _MECANUM_VEHICLE_H_